#include <cusp/linear_operator.h>
#include <cusp/transpose.h>
#include <cusp/gallery/eye.h>
#include <cusp/blas.h>
#include <thrust/detail/type_traits.h>
#include <thrust/pair.h>

namespace cusp
{
namespace detail
{

template <typename ValueType>
__host__ __device__
inline void rotmat(ValueType a, ValueType b,
                   ValueType& c, ValueType& s)
{
	ValueType t;

	if (b == 0)
	{
		c = 1;
		s = 0;
	}
	else if (abs(b) > abs(a))
	{
		t = a/b;
		s = 1 / sqrt(1 + t*t);
		c = t * s;
	}
	else
	{
		t = b/a;
		c = 1 / sqrt(1 + t*t);
		s = t * c;
	}
}

} // end namespace detail

template <typename LinearOperator,
          typename Matrix1,
          typename Matrix2>
void qr(LinearOperator& A,
        Matrix1& Q,
        Matrix2& R)
{
  CUSP_PROFILE_SCOPED();

  typedef typename LinearOperator::value_type   ValueType;
  typedef typename LinearOperator::memory_space MemorySpace;

  typedef typename cusp::array1d<ValueType, MemorySpace>::iterator Iterator;
  typedef typename cusp::array1d_view<Iterator> ArrayView;
  
  Matrix1 Qt;

  R = A;
  cusp::gallery::eye(Qt, A.num_rows, A.num_rows);

  cusp::array1d<ValueType, MemorySpace> tv0(A.num_cols), tv1(A.num_cols);
  cusp::array1d<ValueType, MemorySpace> tv2(Qt.num_cols), tv3(Qt.num_cols);
  for (size_t j = 0; j < R.num_cols; ++j)
  {
    for (size_t i = R.num_rows-1; i > j; --i)
    {
      ArrayView row0(R.values.begin() + (i-1) * R.pitch, R.values.begin() + (i-1) * R.pitch + R.num_cols);
      ArrayView row1(R.values.begin() +  i    * R.pitch, R.values.begin() +  i    * R.pitch + R.num_cols);

      ValueType a = row0[j];
      ValueType b = row1[j];
      ValueType c, s;
      cusp::detail::rotmat(a, b, c, s);

      cusp::blas::axpby(row0, row1, tv0,  c, s);
      cusp::blas::axpby(row0, row1, tv1, -s, c);
      cusp::blas::copy(tv0, row0);
      cusp::blas::copy(tv1, row1);
      row1[j] = 0;

      ArrayView qrow0(Qt.values.begin() + (i-1) * Qt.pitch, Qt.values.begin() + (i-1) * Qt.pitch + Qt.num_cols);
      ArrayView qrow1(Qt.values.begin() +  i    * Qt.pitch, Qt.values.begin() +  i    * Qt.pitch + Qt.num_cols);
      cusp::blas::axpby(qrow0, qrow1, tv2,  c, s);
      cusp::blas::axpby(qrow0, qrow1, tv3, -s, c);
      cusp::copy(tv2, qrow0);
      cusp::copy(tv3, qrow1);
    }
  }

  cusp::transpose(Qt, Q);
}

} // end namespace cusp

